/**************************************************************/
/*  This file is licensed from, and is a trade secret of:     */
/*                                                            */
/*    Mercury Computer Systems, Incorporated                  */
/*    199 Riverneck Road                                      */
/*    Chelmsford, Massachusetts 01824-2820                    */
/*    United States of America                                */
/*    Telephone + 1 978 256-1300                              */
/*                                                            */
/*  Refer to your Software License Agreement for provisions   */
/*  on use, duplication, or disclosure.                       */
/*  US. Government users and prime contractors are also       */
/*  governed by the following Data Rights Notice:             */
/*  Use, duplication or disclosure by the US. Government is   */
/*  subject to the provisions set forth in FAR 52.227-14 for  */
/*  restricted rights or other agency data rights provisions  */
/*  on commercial software and documentation, as applicable.  */
/*                                                            */
/*  This file: (a) was developed at private expense, and no   */
/*  part was developed with government funds, (b) is a        */
/*  trade secret of Mercury Computer Systems, Inc. for the    */
/*  purposes of the Freedom of Information Act, (c) is        */
/*  "commercial computer software" subject to limited         */
/*  utilization as provided in the above noted Software       */
/*  License Agreement and (d) is in all respects data         */
/*  belonging exclusively to Mercury Computer Systems, Inc..  */
/*                                                            */
/**************************************************************/

/******************************************************************/
/* Copyright (c) 1999-2003, Mercury Computer Systems, Inc.,       */
/* Chelmsford MA.,and all third party embedded software sources.  */
/* All rights reserved under the Copyright laws of US. and        */
/* international treaties.                                        */
/*                                                                */
/* Patent Pending                                                 */
/******************************************************************/


//$COPYRIGHT_END


#include <stdlib.h>
#include <stdio.h>
#include <string>
#include "BitPacketizer.h"

extern "C" {
  BITPACKETIZER_EXPORT
  POA_CF::Resource *
  CreateBitPacketizer5to7 (const char * identifier)
  {
    Packetizer5to7 * decoder;

    if (Packetizer5to7::Create (decoder, identifier) != success) {
      return 0;
    }

    return (POA_CF::Resource *) decoder;
  }
}

Packetizer5to7::Packetizer5to7 (const char * identifier)
  : CF__Resource_impl (identifier),
    m_threadrunning (false),
    m_bStop (false)
{
  unsigned int input_buffer_size;
  unsigned int output_buffer_size;

  // Initialize buffers.
  input_buffer_size = 512; // 147 with some breathing room.
  m_input = new char [input_buffer_size];

  output_buffer_size = 512;
  m_output = new char [output_buffer_size];

  // Define ports.
  m_bitpacketizerin = new BitPacketizerInPort_impl (input_buffer_size);
  m_bitpacketizerout = new RsBlockOutPort_impl (m_output, output_buffer_size);
}

Packetizer5to7::~Packetizer5to7 ()
{
  delete m_input;
  delete m_output;
  delete m_bitpacketizerin;
  delete m_bitpacketizerout;
}

status
Packetizer5to7::Create (Packetizer5to7 *& handle,
                        const char * identifier)
{
  Packetizer5to7 * Temp = new Packetizer5to7 (identifier);

  if (!Temp) {
    handle = 0;
    return fail;
  }

  handle = Temp;
  return success;
}

status
Packetizer5to7::Init ()
{
#ifdef MCS_VERBOSE
  std::cout << "Packetizer5to7: Initializing." << std::endl;
#endif

  m_bStop = false;
  m_output_count = 0;
  return success;
}

// Separate thread master function
void
Packetizer5to7::RunWorker (void * param)
{
  Packetizer5to7 * component =
    (Packetizer5to7 *) param;

#ifdef MCS_VERBOSE
  std::cout << "Packetizer5to7: Component is running." << std::endl;
#endif

  component->RunLoop ();

#ifdef MCS_VERBOSE
  std::cout << "Packetizer5to7: Component is stopping." << std::endl;
#endif
}

// Inner loop function that checks for a stop command.
status
Packetizer5to7::RunLoop()
{
  if (Init () != success) {
    return fail;
  }

  while (!m_bStop) {
    Run();
  }

  m_threadrunning = false;
  m_thread.detach();
  return success;
}

void
Packetizer5to7::start ()
  throw (CORBA::SystemException, CF::Resource::StartError)
{
  if (m_threadrunning) {
    return;
  }

  m_threadrunning = true;
  m_thread.start (RunWorker, (void *) this);
  CF__Resource_impl::start();
}

void
Packetizer5to7::stop ()
  throw (CORBA::SystemException, CF::Resource::StopError)
{
  m_bStop = true;
}

CORBA::Object_ptr
Packetizer5to7::getPort (const CORBA::Char * name)
  throw (CORBA::SystemException, CF::PortSupplier::UnknownPort)
{
  std::string temp (name);

  if (temp == "BitPacketizer5to7In") {
    return m_bitpacketizerin->getPort ();
  }
  else if (temp == "BitPacketizer5to7Out") {
    return m_bitpacketizerout->getPort ();
  }
  else {
    throw CF::PortSupplier::UnknownPort ();
  }
}

// Inner loop method that performs component business logic.
status
Packetizer5to7::Run ()
{
  /*
   * Packetizer requests data until at least 147 symbols are ready.
   */

  unsigned int input_count = m_bitpacketizerin->GetData (&m_input);

  if (input_count == 0) {
    return fail;
  }
  else if (input_count < 147) {
    // Not enough data in buffer; release and wait for more.
    m_bitpacketizerin->ReleaseDataBuffer (0);
    return fail;
  }

#ifdef MCS_VERBOSE
  std::cout << "Packetizer5to7: Received " << input_count
	    << " symbols." << std::endl;
#endif

  long startIndex=0;

  while (input_count >= 7) {
    m_output[m_output_count + 0] =
      (m_input[startIndex + 0] << 2) | ((m_input[startIndex + 1] & 0x18 ) >> 3);

    m_output[m_output_count + 1] = 
      ((m_input[startIndex + 1] & 0x07) << 4) | ((m_input[startIndex + 2] & 0x1E) >> 1);

    m_output[m_output_count + 2] = 
      ((m_input[startIndex + 2] & 0x01) << 6) | (m_input[startIndex + 3] << 1) | ((m_input[startIndex + 4] & 0x10) >> 4);

    m_output[m_output_count + 3] = 
      ((m_input[startIndex + 4] & 0x0F) << 3) | ((m_input[startIndex + 5] & 0x1C) >> 2);

    m_output[m_output_count + 4] = 
      ((m_input[startIndex + 5] & 0x03) << 5) | m_input[startIndex + 6];

    startIndex += 7;
    input_count -= 7;
    m_output_count += 5;
  }


  m_bitpacketizerin->ReleaseDataBuffer (startIndex);

  /*
   * Only push data when enough samples have accumulated.
   */

  if (m_output_count >= 105) {
#ifdef MCS_VERBOSE
    std::cout << "Packetizer5to7: Sending " << m_output_count
	      << " samples." << std::endl;
#endif
    m_bitpacketizerout->pushData (m_output, m_output_count);
    m_output_count = 0;
  }

  return success;
}

